
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mavlink_param.c
  * @author     baiyang
  * @date       2021-7-22
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mavlink_param.h"
#include "mavproxy.h"

#include <string.h>

#include <rtdevice.h>
#include <rthw.h>
#include <rtthread.h>

#include <c_library_v2/common/mavlink.h>
#include <c_library_v2/ardupilotmega/mavlink.h>
/*-----------------------------------macro------------------------------------*/
#define MAV_PARAM_COUNT (sizeof(mav_param_list_t) / sizeof(mav_param_t))
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
static mav_param_list_t mavlink_param = {
    MAVLINK_PARAM_DEFINE(FORMAT_VERSION, 120),
    MAVLINK_PARAM_DEFINE(SYSID_THISMAV, 1),
    MAVLINK_PARAM_DEFINE(SYSID_MYGCS, 255),
    MAVLINK_PARAM_DEFINE(COMPASS_USE, 1),
    MAVLINK_PARAM_DEFINE(COMPASS_USE2, 1),
    MAVLINK_PARAM_DEFINE(COMPASS_DEV_ID, 466441),
    MAVLINK_PARAM_DEFINE(COMPASS_DEV_ID2, 131594),
    MAVLINK_PARAM_DEFINE(COMPASS_DEV_ID3, 0),
    MAVLINK_PARAM_DEFINE(COMPASS_PRIMARY, 0),
    MAVLINK_PARAM_DEFINE(COMPASS_OFS_X, -116.7277),
    MAVLINK_PARAM_DEFINE(COMPASS_OFS_Y, -58.45957),
    MAVLINK_PARAM_DEFINE(COMPASS_OFS_Z, -197.4254),
    MAVLINK_PARAM_DEFINE(COMPASS_OFS2_X, -61.33213),
    MAVLINK_PARAM_DEFINE(COMPASS_OFS2_Y, 198.8624),
    MAVLINK_PARAM_DEFINE(COMPASS_OFS2_Z, 215.1222),
    MAVLINK_PARAM_DEFINE(COMPASS_OFS3_X, 0),
    MAVLINK_PARAM_DEFINE(COMPASS_OFS3_Y, 0),
    MAVLINK_PARAM_DEFINE(COMPASS_OFS3_Z, 0),
    MAVLINK_PARAM_DEFINE(BATT_MONITOR, 0),
    MAVLINK_PARAM_DEFINE(ARMING_CHECK, 1),
    MAVLINK_PARAM_DEFINE(INS_ACCOFFS_X, 0.01656544),
    MAVLINK_PARAM_DEFINE(INS_ACCOFFS_Y, -0.0822627),
    MAVLINK_PARAM_DEFINE(INS_ACCOFFS_Z, -1.256726),
    MAVLINK_PARAM_DEFINE(INS_ACCSCAL_X, 0.9906809),
    MAVLINK_PARAM_DEFINE(INS_ACCSCAL_Y, 0.9962569),
    MAVLINK_PARAM_DEFINE(INS_ACCSCAL_Z, 0.9864124),
    MAVLINK_PARAM_DEFINE(FLTMODE1, 0),
    MAVLINK_PARAM_DEFINE(FLTMODE2, 0),
    MAVLINK_PARAM_DEFINE(FLTMODE3, 0),
    MAVLINK_PARAM_DEFINE(FLTMODE4, 0),
    MAVLINK_PARAM_DEFINE(FLTMODE5, 0),
    MAVLINK_PARAM_DEFINE(FLTMODE6, 0),
    MAVLINK_PARAM_DEFINE(MNT_RC_IN_ROLL, 0),
    MAVLINK_PARAM_DEFINE(MNT_RC_IN_PAN, 0),
    MAVLINK_PARAM_DEFINE(MNT_RC_IN_TILT, 0),
};

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
static int _mav_param_get_index(const mav_param_t* param)
{
    mav_param_t* mav_param = (mav_param_t*)&mavlink_param;

    return param - mav_param;
}

static mav_param_t* _mav_param_get_by_index(uint32_t index)
{
    mav_param_t* mav_param = (mav_param_t*)&mavlink_param;

    return &mav_param[index];
}

static void _mav_param_pack(mavlink_message_t* msg_t, const mav_param_t* param)
{
    mavlink_param_value_t mav_param_value;
    uint16_t len = strlen(param->name);
    mavlink_system_t mavlink_system = mavproxy_get_system();

    mav_param_value.param_count = MAV_PARAM_COUNT + param_get_count();
    mav_param_value.param_index = _mav_param_get_index(param);
    memset(mav_param_value.param_id, 0, 16);
    memcpy(mav_param_value.param_id, param->name, len < 16 ? len : 16);

    mav_param_value.param_type = MAVLINK_TYPE_FLOAT;
    mav_param_value.param_value = param->value;

    mavlink_msg_param_value_encode(mavlink_system.sysid, mavlink_system.compid, msg_t, &mav_param_value);
}

static void _param_pack(mavlink_message_t* msg_t, const param_t* param)
{
    mavlink_param_value_t mav_param_value;
    uint16_t len = strlen(param->name);
    mavlink_system_t mavlink_system  = mavproxy_get_system();

    mav_param_value.param_count = MAV_PARAM_COUNT + param_get_count();
    mav_param_value.param_index = MAV_PARAM_COUNT + param_get_index(param);
    memset(mav_param_value.param_id, 0, 16);
    memcpy(mav_param_value.param_id, param->name, len < 16 ? len : 16);

    switch (param->type) {
    case PARAM_TYPE_FLOAT:
        mav_param_value.param_type = MAVLINK_TYPE_FLOAT;
        mav_param_value.param_value = param->val.f;
        break;

    case PARAM_TYPE_INT32:
        mav_param_value.param_type = MAVLINK_TYPE_INT32_T;
        mav_param_value.param_value = param->val.i32;
        break;

    case PARAM_TYPE_UINT32:
        mav_param_value.param_type = MAVLINK_TYPE_UINT32_T;
        mav_param_value.param_value = param->val.u32;
        break;

    case PARAM_TYPE_INT16:
        mav_param_value.param_type = MAVLINK_TYPE_INT16_T;
        mav_param_value.param_value = param->val.i16;
        break;

    case PARAM_TYPE_UINT16:
        mav_param_value.param_type = MAVLINK_TYPE_UINT16_T;
        mav_param_value.param_value = param->val.u16;
        break;

    case PARAM_TYPE_INT8:
        mav_param_value.param_type = MAVLINK_TYPE_INT8_T;
        mav_param_value.param_value = param->val.i8;
        break;

    case PARAM_TYPE_UINT8:
        mav_param_value.param_type = MAVLINK_TYPE_UINT8_T;
        mav_param_value.param_value = param->val.u8;
        break;

    default:
        mav_param_value.param_type = MAVLINK_TYPE_FLOAT;
        mav_param_value.param_value = param->val.f;
        break;
    }

    mavlink_msg_param_value_encode(mavlink_system.sysid, mavlink_system.compid, msg_t, &mav_param_value);
}

void send_mavlink_param(char* name)
{
    mav_param_t* mav_param;
    mavlink_message_t msg;

    for (uint32_t i = 0; i < MAV_PARAM_COUNT; i++) {
        mav_param = _mav_param_get_by_index(i);

        if (!mav_param) {
            continue;
        }

        if (strcmp(mav_param->name, name) == 0) {
            _mav_param_pack(&msg, mav_param);
            mavproxy_send_immediate_msg(&msg, 1);
            break;
        }
    }
}

void mavlink_param_send_all(void)
{
    mavlink_message_t msg;
    param_t* param;
    param_group_t* gp = (param_group_t*)&param_list;
    mav_param_t* mav_param;

    for (uint32_t i = 0; i < MAV_PARAM_COUNT; i++) {
        mav_param = _mav_param_get_by_index(i);

        if (!mav_param) {
            break;
        }

        _mav_param_pack(&msg, mav_param);
        mavproxy_send_immediate_msg(&msg, 1);
        rt_thread_mdelay(2);    // the delay is needed because TELEM has relatively high transfer latency
    }

    for (uint32_t i = 0; i < sizeof(param_list_t) / sizeof(param_group_t); i++) {
        param = gp->content;

        for (uint32_t j = 0; j < gp->param_num; j++) {
            _param_pack(&msg, param);
            mavproxy_send_immediate_msg(&msg, 1);
            rt_thread_mdelay(2);    // the delay is needed because TELEM has relatively high transfer latency

            param++;
        }

        gp++;
    }
}

gp_err mavlink_param_set(const char* name, float val)
{
    param_t* param;
    param = param_get_by_name(name);

    if (param == NULL) {
        return GP_ERROR;
    }

    param_set_val(param, &val);

    return GP_EOK;
}

gp_err mavlink_param_send(const param_t* param)
{
    mavlink_message_t msg;

    _param_pack(&msg, param);
    mavproxy_send_immediate_msg(&msg, 1);

    return GP_EOK;
}

/*------------------------------------test------------------------------------*/


